# -*- encoding: utf-8 -*-
"""
@File    :   obstacle.py

@Contact :   2055466817@qq.com

@Modify Time :   2020/12/2 上午7:24

@Author :   赵方国
"""
from naoqi import ALProxy
import cv2 as cv
import random
from cmath import pi
import time


def obstacle(robotIP, PORT, angleINIT, flag):
    """
    简单的执行绕过障碍物的动作，走的是方波形式

    @param robotIP:
    @param PORT:
    @param angleINIT:传入初始角度用于校准
    @param flag: flag为1时，向右侧行走，为-1时，向左侧行走
    @return:
    """
    smallTurnStep = [["StepHeight", 0.01], ["MaxStepX", 0.03]]  # 单步移动
    lower_steps = [["LeftStepHeight", 0.025], ["RightStepHeight", 0.025], ["MaxStepX", 0.05]]  # 连续走动

    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)
    # 绕开障碍物
    # motionProxy.moveTo(0.2, -0.2, -pi / 2, smallTurnStep)  # TODO 此处的值需要结合阈值进行调整

    motionProxy.moveTo(0, 0, flag * (-pi / 2), smallTurnStep)

    correctAngle(robotIP, PORT, angleINIT)

    motionProxy.moveTo(0.35, 0, 0, smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    motionProxy.moveTo(0, 0, flag * (pi / 2), smallTurnStep)
    correctAngle(robotIP, PORT, angleINIT)
    # motionProxy.moveTo(0.65, 0, 0, smallTurnStep)
    motionProxy.moveTo(0.2, 0, 0, smallTurnStep)
    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    motionProxy.moveTo(0.2, 0, 0, smallTurnStep)

    motionProxy.moveTo(0, 0, 0, smallTurnStep)
    motionProxy.moveTo(0.2, 0, 0, smallTurnStep)

    # 进行角度的校准
    correctAngle(robotIP, PORT, angleINIT)
    # 绕开障碍物
    motionProxy.moveTo(0, 0, flag * (pi / 6), smallTurnStep)
    #
    # correctAngle(robotIP, PORT, angleINIT)
    # motionProxy.moveTo(0.35, 0, 0, smallTurnStep)
    # correctAngle(robotIP, PORT, angleINIT)
    # motionProxy.moveTo(0, 0, flag * (-pi / 2), smallTurnStep)
    #
    # # 进行角度的校准
    # correctAngle(robotIP, PORT, angleINIT)
    tts.say("finish the obstacle")


def correctAngle(robotIP, PORT, angleINIT):
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    angle = valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    # print 'angle: ' + str(angle), 'trurned: ' + str(-(angle - angleINIT))
    print "current angle:" + str(angle)
    angleREcorrect = (angle - angleINIT)
    print "angleRECORRECT:" + str(angleREcorrect)

    if abs(angleREcorrect) < 1 * pi / 180:
        print "do not need to correct"

        return True

    if abs(angleREcorrect) > pi:

        if angleREcorrect > 0:
            angleCorrect = abs(angle - angleINIT) - pi - pi / 2
        else:
            angleCorrect = -(abs(angle - angleINIT) - pi - pi / 2)
        print "大于 pi，校准"
        print "turn to " + str(- angleCorrect)
        motionProxy.moveTo(0, 0, -angleCorrect)

        return True

    if 1 * pi / 180 < abs(angleREcorrect) < 49 * pi / 180:
        print '直线校准'
        print str(-(angle - angleINIT))
        motionProxy.moveTo(0, 0, -(angle - angleINIT))

        return True

    if abs(angleREcorrect) > 93 * pi / 180 or ((87 * pi / 180) > abs(angleREcorrect) > (50 * pi / 180)):

        print "不标准的90度，校准"
        print 'turn to ' + str((abs(angleREcorrect) - 90 * pi / 180))
        if angleREcorrect < 0:
            motionProxy.moveTo(0, 0, (abs(angleREcorrect) - 90 * pi / 180))
        else:
            motionProxy.moveTo(0, 0, -(abs(angleREcorrect) - 90 * pi / 180))

        return True


if __name__ == '__main__':
    robotIP = "169.254.119.52"
    PORT = 9559
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)
    valueProxy = ALProxy("ALMemory", robotIP, PORT)
    tts = ALProxy("ALTextToSpeech", robotIP, PORT)
    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    obstacle(robotIP, 9559, valueProxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
